#include <automatic/automatic.h>

/*
 * selectBehavior.cpp
 *
 *	Select Behavior will select the behavior to let act
 *	at this time step. Behaviors are filtered by active
 *	status, and of the active behaviors, each behavior's
 *	probability to act is directly related to its priority.
 *
 *	The exception are behaviors with priority 1. This is taken
 *	to be an EXCLUSIVE behavior, which will ALWAYS act, except
 *	when another exclusive behavior is selected first.
 *
 *  Created on: Oct 22, 2012
 *      Author: Matthijs
 */
Behavior * Automatic::selectBehavior ()
{
	double topScore = 0.0;
	int winner = 0;
	for (uint i = 0; i < behaviors.size(); i++)
	{
		if (!behaviors[i] -> isActive())
			continue;

		double priority = behaviors[i] -> getPriority();

//		ROS_ERROR("Priority for %d: %f", i, priority);
		if (priority > topScore)
		{
			topScore = priority;
			winner   = i;
		}
		if (priority == 1.0)
			return behaviors[i];
	}

//	ROS_ERROR("Winner is %d", winner);
	return behaviors[winner];
}

